#include "./debug_io.h"
#include "../compat.h"

static GPIO_TypeDef* const Ports[] = {
    GPIOA,
    GPIOB,
    GPIOC,
    GPIOD,
    GPIOE,
};

static const uint32_t PortRccFields[] = {
    RCC_APB2ENR_IOPAEN,
    RCC_APB2ENR_IOPBEN,
    RCC_APB2ENR_IOPCEN,
    RCC_APB2ENR_IOPDEN,
    RCC_APB2ENR_IOPEEN,
};

void debug_io_ctrl(uint8_t port, uint8_t pin, uint8_t level) {
    uint8_t max = sizeof (Ports) / sizeof (Ports[0]);
    if ((port >= max) || (pin >= 16)) {
        return;
    }
    RCC->APB2ENR |= PortRccFields[port];

    if (port >= 8) {
        uint32_t mask = 0x0F << ((pin - 8) * 4);
        uint32_t set = 0x03 << ((pin - 8) * 4);
        Ports[port]->CRH = Ports[port]->CRH & (~mask) | set;
    } else {
        uint32_t mask = 0x0F << ((pin) * 4);
        uint32_t set = 0x03 << ((pin) * 4);
        Ports[port]->CRL = Ports[port]->CRL & (~mask) | set;
    }

    if (level == 0) {
        Ports[port]->BRR = 0x01 << pin;
    } else {
        Ports[port]->BSRR = 0X01 << pin;
    }
}
